#include <Arduino.h>
#include <Wire.h>

//  ----  ----
#include "INA226.h"
INA226 ina(Wire);
//  ----  ----
#include "TFT_eSPI.h"
TFT_eSPI tft = TFT_eSPI(80, 160);
//  ----  ----

//#define debug
#define DP 25
#define DN 26
float v_DN,v_DP;

//  ----  ----
typedef struct Pages {
    struct Pages *prev;
    struct Pages *next;
    const String Page_name;
    bool submenu;
    bool sub_conf;
    int Page_Val;

    void (*page_op)(void);
    void (*submenu_op)(void);
}Page;
void page1();
Page Page1={NULL,NULL,"Present Power",0,0,0,(*page1),NULL};

//    ----  Global Variable ----
bool current_dir(0);
int KeyVal(0);
float Shuntvoltage(0),Busvoltage(0),Current_mA(0),Loadvoltage(0);
float mah(0),mwh(0);
int32_t time_last(0),tm_last(0);

//    ----  Functions ----
void espDelay(int ms)
{
  esp_sleep_enable_timer_wakeup(ms * 1000);
  esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_ON);
  esp_light_sleep_start();
}

void ina226_readvalues() 
{
  int32_t time_now = millis();
  Shuntvoltage = ina.readShuntVoltage();
  Busvoltage = ina.readBusVoltage();
  Current_mA = ina.readShuntCurrent() * 1000;
  Loadvoltage = Busvoltage + (Shuntvoltage / 1000);

  if (Loadvoltage < 0)  Loadvoltage = - Loadvoltage;
  if (Current_mA < 0)   
  {
    Current_mA = -Current_mA;
    current_dir = 1;
  }
  else current_dir = 0;

  if ((time_now - time_last >= 50) && (Current_mA >= 5))
  {
    mah = mah + (Current_mA * ((time_now - time_last)) / 1000)/3600;
    mwh = mwh + (Loadvoltage * ((Current_mA * ((time_now - time_last)) / 1000)/3600));
    time_last = time_now;
  }

  #ifdef debug
  Serial.print("Voltage:");
  Serial.print(Loadvoltage,2);
  Serial.println("V");
  Serial.print("Current:");
  Serial.print(Current_mA,2);
  Serial.println("mA");
  #endif
}

void usb_DB_readvol()
{
  //if(WiFi.getMode() != WIFI_MODE_NULL)  WiFi.disconnect(true,true);
  adcAttachPin(DP);
  v_DP = analogReadMilliVolts(DP)/1000;
  adcAttachPin(DN);
  v_DN = analogReadMilliVolts(DN)/1000;
}

void page1()
{
  int i,n;
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE); 
  tft.setTextSize(3);
  if(Loadvoltage<10) tft.setCursor(50,0);
  else tft.setCursor(32,0);
  tft.print(Loadvoltage,3);
  tft.print("V");

  tft.setCursor(50,25);
  tft.print(Current_mA/1000,3);
  tft.print("A");

  float power = Loadvoltage*Current_mA/1000 ;
  if(power < 10) {
    tft.setCursor(50,50);
    tft.print(power,3);
  }
  else {  
    if(power < 100) {
      tft.setCursor(32,50);
      tft.print(power,3);
    }
    else {
      tft.setCursor(32,50);
      tft.print(power,2);
    }
  }
  tft.print("W");

  tft.setTextSize(1);
  if(mah<1000) 
  {
    tft.setCursor(0,0);
    tft.print("mAh:");
    tft.setCursor(0,10);
    tft.print(mah,1);
  }
  else {
    if(mah > 9999000)
    {
      tft.setCursor(0,0);
      tft.print("Ah:");
      tft.setCursor(0,10);
      tft.print("9999");
    }
    else{
      i = mah/1000;
      n = 1;
      while(i>10)
      {
       i = i /10;
       n += 1;
      }
      tft.setCursor(0,0);
      tft.print("Ah:");
      tft.setCursor(0,10);
      tft.print(mah/1000,4-n);
    }
  }
    
  if(mwh<1000) 
  {
    tft.setCursor(0,20);
    tft.print("mWh:");
    tft.setCursor(0,30);
    tft.print(mwh,1);
  }
  else {
    if(mwh > 9999000)
    {
      tft.setCursor(0,20);
      tft.print("Wh:");
      tft.setCursor(0,30);
      tft.print("9999");
    }
    else{
      i = mwh/1000;
      n = 1;
      while(i>10)
      {
        i = i /10;
        n += 1;
      }
      tft.setCursor(0,20);
      tft.print("Wh:");
      tft.setCursor(0,30);
      tft.print(mwh/1000,4-n);
    }
    
  }
  
  //
  tft.setTextSize(1);
  tft.setCursor(0,40);
  tft.print("D+:");
  tft.setCursor(0,50);
  tft.print(v_DP,3);
  tft.setCursor(0,60);
  tft.print("D-:");
  tft.setCursor(0,70);
  tft.print(v_DN,3);

  #ifdef debug
  Serial.print("D+:");
  Serial.println(v_DP);
  Serial.print("D-:");
  Serial.println(v_DN);
  #endif
}

//----                ----
//    ----  Setup ----
//----                ----

void setup() 
{
  #ifdef debug
  Serial.begin(115200);
  #endif

  tft.begin();           //turn on display
  tft.setRotation(3);

  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_WHITE); 
  tft.setTextSize(2);
  tft.setCursor(0, 30);
  tft.print("Initializing");

  pinMode(DN,INPUT);
  pinMode(DP,INPUT);

  #ifdef debug
  Serial.println("Initialize INA226");
  Serial.println("-----------------------------------------------");
  #endif
  Wire.begin();

  if(!ina.begin())
  {
    #ifdef debug
    Serial.println("INA226 Connection Error");
    #endif
    tft.fillScreen(TFT_BLACK);
    tft.setTextColor(TFT_WHITE); 
    tft.setTextSize(5);
    tft.setCursor(20, 20);
    tft.print("INA226 Connection Error");
    espDelay(5000);
    while(1){
      espDelay(1000);
    }
  }

  // Configure INA226
  ina.configure(INA226_AVERAGES_1, INA226_BUS_CONV_TIME_1100US, INA226_SHUNT_CONV_TIME_1100US, INA226_MODE_SHUNT_BUS_CONT);
  ina.calibrate(0.01, 5);   // Calibrate INA226. Rshunt = 0.01 ohm, Max excepted current = 5A
  ina.enableOverPowerLimitAlert();
  ina.setPowerLimit(0.10);
  ina.setAlertLatch(true);

  #ifdef debug
  Serial.println("Initlize Done");
  Serial.println("-----------------------------------------------");
  #endif
  tm_last = millis();
}

void loop()
{
  ina226_readvalues();
  usb_DB_readvol();
  if((millis()-tm_last) >=500 )
  {
    page1();
    tm_last = millis();
  }
  espDelay(50);
}
